import Joint from './joint';
import CONSTANT from '../../constant';
import Method from "../../common/method";

export default class SphericalJoint extends Joint{
    constructor(config) {
        super(config, CONSTANT.JOINT_TYPE_SPHERICAL);
        this._sd = config.springDamper.clone();
    }
    getInfo(info,timeStep,isPositionPart) {
        const SDR=CONSTANT.SETTING_MIN_SPRING_DAMPER_DAMPING_RATIO;
        if(this._sd.frequency > 0 && isPositionPart) {
            return;
        }
        let tv=this._tv;
        Method.subArray(this.anchor2,this.anchor1,tv,0,0,0,3);
        let errorX=tv[0], errorY=tv[1], errorZ=tv[2];
        let cfm;
        let erp;
        if(this._sd.frequency > 0) {
            let omega = 6.28318530717958 * this._sd.frequency;
            let zeta = this._sd.dampingRatio;
            if(zeta < SDR) {
                zeta = SDR;
            }
            let h = timeStep.dt;
            let c = 2 * zeta * omega;
            let k = omega * omega;
            if(this._sd.useSymplecticEuler) {
                cfm = 1 / (h * c);
                erp = k / c;
            } else {
                cfm = 1 / (h * (h * k + c));
                erp = k / (h * k + c);
            }
            cfm *= this._b1._invMass + this._b2._invMass;
        } else {
            cfm = 0;
            erp = this.getErp(timeStep,isPositionPart);
        }
        let linearRhsX = errorX * erp,linearRhsY = errorY * erp,linearRhsZ = errorZ * erp;
        let ra1=this.relativeAnchor1,ra2=this.relativeAnchor2;
        let c100 = 0,c101 = ra1[2],c102 = -ra1[1];
        let c110 = -ra1[2],c111 = 0,c112 = ra1[0];
        let c120 = ra1[1],c121 = -ra1[0],c122 = 0;
        let c200 = 0,c201 = ra2[2],c202 = -ra2[1];
        let c210 = -ra2[2],c211 = 0,c212 = ra2[0];
        let c220 = ra2[1],c221 = -ra2[0],c222 = 0;
        let impulse = this._impulses[0];
        let row = info.rows[info.numRows++];
        this._resetRow(row,impulse);
        row.rhs = linearRhsX;
        row.cfm = cfm;
        row.minImpulse = -1e65536;
        row.maxImpulse = 1e65536;
        Method.setJacobianElements(row.jacobian.elements,1,0,0,1,0,0,c100,c101,c102,c200,c201,c202);
        let impulse1 = this._impulses[1];
        let row1 = info.rows[info.numRows++];
        this._resetRow(row1,impulse1);
        row1.rhs = linearRhsY;
        row1.cfm = cfm;
        row1.minImpulse = -1e65536;
        row1.maxImpulse = 1e65536;
        Method.setJacobianElements(row1.jacobian.elements,0,1,0,0,1,0,c110,c111,c112,c210,c211,c212);
        let impulse2 = this._impulses[2];
        let row2 = info.rows[info.numRows++];
        this._resetRow(row2,impulse2);
        row2.rhs = linearRhsZ;
        row2.cfm = cfm;
        row2.minImpulse = -1e65536;
        row2.maxImpulse = 1e65536;
        Method.setJacobianElements(row2.jacobian.elements,0,0,1,0,0,1,c120,c121,c122,c220,c221,c222);
    }
    _getVelocitySolverInfo(timeStep,info) {
        super._getVelocitySolverInfo(timeStep,info);
        this.getInfo(info,timeStep,false);
    }
    _getPositionSolverInfo(info) {
        super._getPositionSolverInfo(info);
        this.getInfo(info,null,true);
    }
    getSpringDamper() {
        return this._sd;
    }
}